#pragma once
#include <xmmintrin.h>
#include <emmintrin.h>
#include <tmmintrin.h>
#include <intrin.h>
#include <math.h>

#include "vector.h"

#pragma intrinsic (_mm_hadd_ps)

static const unsigned long posMask = 0x00000000;
static const unsigned long negMask = 0x80000000;

#define _MM_SHUFFLE_SELF(xmm, a, b, c, d) (__m128&)_mm_shuffle_epi32((__m128i&)(xmm), _MM_SHUFFLE((a), (b), (c), (d)))

namespace sse_math
{
#define EPSILON 0.00000000001f
#define PIOVER180 0.017453292519943295769236907684886f

	class CQuaternion
	{
	private:
		__m128 _q; // x, y, z, w
	public:
		CQuaternion(){_q = _mm_setr_ps(0.0f, 0.0f, 0.0f, 1.0f);}
		CQuaternion(const __m128& q):_q(q){}
		CQuaternion(const CQuaternion& q):_q(q._q){}
		CQuaternion(const float x, const float y, const float z, const float w){_q = _mm_setr_ps(x, y, z, w);}

		__inline const __m128& getXMM(void) const {return _q;}
		__inline __m128& getXMM(void) {return _q;}
		/* Conjugate */
		__inline CQuaternion getConjugate() const
		{
			static const __m128 conjMask = _mm_setr_ps(*(float*)&negMask, *(float*)&negMask, *(float*)&negMask, *(float*)&posMask);
			return CQuaternion(_mm_xor_ps(_q, conjMask));
		}

		/* Normalize */
		__inline CQuaternion& normalize()
		{
			__m128 d;
			d = _mm_mul_ps(_q, _q);
			d = _mm_hadd_ps(d, d);
			d = _mm_hadd_ps(d, d);

			float mag2 = d.m128_f32[0];
			if (fabs(mag2) > EPSILON)
				*this /= sqrt(mag2);
			return *this;
		}

		__inline CQuaternion& operator = (const CQuaternion& q) {_q = q._q; return *this;}

		__inline CQuaternion operator* (const CQuaternion& q) const
		{
			__m128 qx;
			__m128 qy;
			__m128 qz;
			__m128 qw;

			qw = _mm_mul_ps(q._q, (__m128&)_mm_shuffle_epi32((__m128i&)_q, _MM_SHUFFLE(3, 3, 3, 3)));

			qx = _mm_mul_ps(q._q, (__m128&)_mm_shuffle_epi32((__m128i&)_q, _MM_SHUFFLE(0, 0, 0, 0)));
			qx = (__m128&)_mm_shuffle_epi32((__m128i&)qx, _MM_SHUFFLE(0, 1, 2, 3));

			qy = _mm_mul_ps(q._q, (__m128&)_mm_shuffle_epi32((__m128i&)_q, _MM_SHUFFLE(1, 1, 1, 1)));
			qy = (__m128&)_mm_shuffle_epi32((__m128i&)qy, _MM_SHUFFLE(1, 0, 3, 2));

			qz = _mm_mul_ps(q._q, (__m128&)_mm_shuffle_epi32((__m128i&)_q, _MM_SHUFFLE(2, 2, 2, 2)));
			qz = (__m128&)_mm_shuffle_epi32((__m128i&)qz, _MM_SHUFFLE(2, 3, 0, 1));

			static const __m128 sign1 = _mm_setr_ps(*(float*)&posMask, *(float*)&negMask, *(float*)&posMask, *(float*)&negMask);
			static const __m128 sign2 = _mm_setr_ps(*(float*)&posMask, *(float*)&posMask, *(float*)&negMask, *(float*)&negMask);
			static const __m128 sign3 = _mm_setr_ps(*(float*)&negMask, *(float*)&posMask, *(float*)&posMask, *(float*)&negMask);

			qx = _mm_xor_ps(qx, sign1);
			qy = _mm_xor_ps(qy, sign2);
			qz = _mm_xor_ps(qz, sign3);

			return CQuaternion(_mm_add_ps(qw, _mm_add_ps(qx, _mm_add_ps(qy, qz))));
		}

		__inline CVector3 operator* (const CVector3& v) const
		{
			CVector3 vv(v);
			CQuaternion vecQuat(vv.normalize().getXMM());

			CQuaternion& resQuat = *this * vecQuat * getConjugate();

			return (CVector3(resQuat.getXMM()));
		}

		__inline CQuaternion& FromAxis(const CVector3& v, float theta)
		{
			float sinAngle;
			theta *= 0.5f;

			sinAngle = sin(theta);
			CVector3 vv(v);
			__m128 u = vv.normalize().getXMM();

			_q = _mm_mul_ps(u, _mm_load1_ps(&sinAngle));
			_q.m128_f32[3] = cos(theta);

			return *this;
		}

		/*
		replaces a call to
		CMatrix& m = _orientation.getMatrix().transpose();
		_posistion = CVector(-m.z1, -m.z2, -m.z3);
		*/

		CVector3 getDirectionVector() const
		{
			__m128 xmm1, xmm2, xmm3;
			float f = -2.0f;
			static const __m128 nppn = _mm_setr_ps(*(float*)&negMask, *(float*)&posMask, *(float*)&posMask, *(float*)&negMask);

			xmm1 = _mm_mul_ps(_MM_SHUFFLE_SELF(_q, 3, 0, 1, 0), _MM_SHUFFLE_SELF(_q, 3, 0, 2, 2));
			xmm2 = _mm_mul_ps(_MM_SHUFFLE_SELF(_q, 3, 1, 3, 3), _MM_SHUFFLE_SELF(_q, 3, 1, 0, 1));

			xmm3 = _mm_mul_ps(_mm_load_ps1(&f), _mm_add_ps(xmm1, _mm_xor_ps(xmm2, nppn)));

			xmm3.m128_f32[2] = -(1.0f + xmm3.m128_f32[2]);

			return CVector3(xmm3);
		}

		/* Addition */
		__inline CQuaternion& operator += (const CQuaternion& q) {_q = _mm_add_ps(_q, q._q); return *this;}
		__inline CQuaternion& operator += (const float f) {_q = _mm_add_ps(_q, _mm_load_ps1(&f)); return *this;}
		__inline CQuaternion operator + (const CQuaternion& q) const {return (CQuaternion(_mm_add_ps(_q, q._q)));}
		__inline CQuaternion operator + (const float f) const {return (CQuaternion(_mm_add_ps(_q, _mm_load_ps1(&f))));}

		/* Substraction */
		__inline CQuaternion& operator -= (const CQuaternion& q) {_q = _mm_sub_ps(_q, q._q); return *this;}
		__inline CQuaternion& operator -= (const float f) {_q = _mm_sub_ps(_q, _mm_load_ps1(&f)); return *this;}
		__inline CQuaternion operator - (const CQuaternion& q) const {return (CQuaternion(_mm_sub_ps(_q, q._q)));}
		__inline CQuaternion operator - (const float f) const {return (CQuaternion(_mm_sub_ps(_q, _mm_load_ps1(&f))));}

		/* Multiplication */
		__inline CQuaternion& operator *= (const float f) {_q = _mm_mul_ps(_q, _mm_load_ps1(&f)); return *this;}
		__inline CQuaternion operator * (const float f) const {return (CQuaternion(_mm_mul_ps(_q, _mm_load_ps1(&f))));}

		/* Division */
		__inline CQuaternion& operator /= (const float f) {_q = _mm_div_ps(_q, _mm_load_ps1(&f)); return *this;}
		__inline CQuaternion operator / (const float f) const {return (CQuaternion(_mm_div_ps(_q, _mm_load_ps1(&f))));}
	};
}